// -*- C++ -*-
/*!
 * @file  MapManagement.cpp
 * @brief Map Management
 * @date $Date$
 *
 * $Id$
 */

#include "MapManagement.h"
#include "map.h"

// Module specification
// <rtc-template block="module_spec">
static const char* mapmanagement_spec[] =
  {
    "implementation_id", "MapManagement",
    "type_name",         "MapManagement",
    "description",       "Map Management",
    "version",           "0.2.1",
    "vendor",            "Tsuyoshi Suenaga, NAIST",
    "category",          "Map",
    "activity_type",     "PERIODIC",
    "kind",              "DataFlowComponent",
    "max_instance",      "1",
    "language",          "C++",
    "lang_type",         "compile",
    // Configuration variables
    "conf.default.map_format", "OCCUPANCY_GRID_MAP",
    "conf.default.map_filename", "mapdata/RTCcenterUnifiedMap.pgm",
    "conf.default.resolution_m", "0.03",

    ""
  };
// </rtc-template>

MapManagement::MapManagement(RTC::Manager* manager)
    // <rtc-template block="initializer">
  : RTC::DataFlowComponentBase(manager),
    m_MapInfoPort("MapInfo"),
    // </rtc-template>
    m_Map(&map),
	dummy(0)
{
  // Registration: InPort/OutPort/Service
  // <rtc-template block="registration">
  // Set InPort buffers
  
  // Set OutPort buffer
  
  // Set service provider to Ports
  m_MapInfoPort.registerProvider("Map", "MapInfo", m_Map);
  
  // Set service consumers to Ports
  
  // Set CORBA Service Ports
  registerPort(m_MapInfoPort);
  
  // </rtc-template>

}

MapManagement::~MapManagement()
{
}



RTC::ReturnCode_t MapManagement::onInitialize()
{
  // <rtc-template block="bind_config">
  // Bind variables and configuration variable
  bindParameter("map_format", m_map_format, "OCCUPANCY_GRID_MAP");
  bindParameter("map_filename", m_map_filename, "mapdata/RTCcenterUnifiedMap.pgm");
  //  bindParameter("map_filename", m_map_filename, "mapdata/sample1.pgm");
  bindParameter("resolution_m", m_resolution_m, "0.03");
  //  bindParameter("resolution_m", m_resolution_m, "0.1");
  
  // 初期化
  map.pmap = NULL;
  map.lmap = NULL;
  map.ogmap = NULL;
  
  // </rtc-template>
  return RTC::RTC_OK;
}

/*
RTC::ReturnCode_t MapManagement::onFinalize()
{
  return RTC::RTC_OK;
}
*/

/*
RTC::ReturnCode_t MapManagement::onStartup(RTC::UniqueId ec_id)
{
  return RTC::RTC_OK;
}
*/

/*
RTC::ReturnCode_t MapManagement::onShutdown(RTC::UniqueId ec_id)
{
  return RTC::RTC_OK;
}
*/


RTC::ReturnCode_t MapManagement::onActivated(RTC::UniqueId ec_id)
{
  // マップ読み込み
  if(!m_map_format.find("POINT_MAP")){
    // ポイントマップ
    //load_point_map("mapdata/pointmap_5f_cm.dat", &map->pmap, 0.01); // [cm] -> [m]
    load_point_map(m_map_filename.c_str(), map.pmap, 0.01); // [cm] -> [m]
  }else if(!m_map_format.find("LINE_MAP")){
    // ラインマップ
    //load_line_map("mapdata/linemap_5f_cm.dat", &map->lmap, 0.01); // [cm] -> [m]
    load_line_map(m_map_filename.c_str(), map.lmap, 0.01); // [cm] -> [m]
    //convert_linemap_to_pointmap(map.lmap ,map.pmap, 0.07); // [m/grid]
  }else if(!m_map_format.find("OCCUPANCY_GRID_MAP")){
    // 占有グリッドマップ
    //load_occupancy_grid_map("mapdata/NAIST1F-occupancyGridMap.pgm", &map->ogmap);
    map.ogmap = load_occupancy_grid_map(m_map_filename.c_str(), map.ogmap);
    //convert_occupancy_gridmap_to_pointmap(map.ogmap, map.pmap, 200);
  }else{
    return RTC::RTC_ERROR;
  }

  return RTC::RTC_OK;
}



RTC::ReturnCode_t MapManagement::onDeactivated(RTC::UniqueId ec_id)
{
  release_maps(&map);

  return RTC::RTC_OK;
}


/*
RTC::ReturnCode_t MapManagement::onExecute(RTC::UniqueId ec_id)
{
  return RTC::RTC_OK;
}
*/

/*
RTC::ReturnCode_t MapManagement::onAborting(RTC::UniqueId ec_id)
{
  return RTC::RTC_OK;
}
*/

/*
RTC::ReturnCode_t MapManagement::onError(RTC::UniqueId ec_id)
{
  return RTC::RTC_OK;
}
*/

/*
RTC::ReturnCode_t MapManagement::onReset(RTC::UniqueId ec_id)
{
  return RTC::RTC_OK;
}
*/

/*
RTC::ReturnCode_t MapManagement::onStateUpdate(RTC::UniqueId ec_id)
{
  return RTC::RTC_OK;
}
*/

/*
RTC::ReturnCode_t MapManagement::onRateChanged(RTC::UniqueId ec_id)
{
  return RTC::RTC_OK;
}
*/



extern "C"
{
 
  void MapManagementInit(RTC::Manager* manager)
  {
    RTC::Properties profile(mapmanagement_spec);
    manager->registerFactory(profile,
                             RTC::Create<MapManagement>,
                             RTC::Delete<MapManagement>);
  }
  
};


